#include <iostream>
#include <cmath>
#include <cstdio>

using namespace std;

int main() {

    int n = 0; //记录执行次数

    double curPosition = 0; //当前位置
    double speed = 0; //小车速度
    double speedLast = 0;//上一次速度

    double targetPosition = 1000; //目标距离
    double maxSpeed = 200; //最大速度限制

    double err = 0; //当前误差
    double errLast = 0; //上次误差
    double errSum = 0; //累计误差

    double kp = 0.5; //比例调节系数
    double ki = 0.000002; //积分调节系数
    double kd = 0.01; //微分调节系数

    while(1) {
        //计算当前距离与目标距离的误差
        err = targetPosition - curPosition;
        //距离根据pid公式调节速度
        speed = err * kp + errSum * ki + (err - errLast) * kd;

        //速度限幅
        if (speed > maxSpeed) {
            speed = maxSpeed;
        } else if (speed < -maxSpeed) {
            speed = -maxSpeed;
        }else {
            //如果速度没有超过最大速度才进行误差累积

        }

        errLast = err; //保存上一次误差值
        errSum += err; //累计误差
        n++; //执行次数加一

        //如果足以克服摩擦力
        if(fabs(speed) > 0.5){
            //根据当前速度前进一段距离
            curPosition += speed * 0.01;
        }

        printf("%4d    |    %4.3f    |    %3.3f    |    %3.3f\n", n, curPosition, speed, speed-speedLast);
        speedLast = speed;

        //打印8000条信息
        if(n > 8000){
            break;
        }

    }

    return 0;
}
